% [f_xnext,f_dxn_dx,f_dxn_du,n,m,N,alphac,epsilon,baru,T,deltat,mu_e,mu_m,mu,r_lagrange_1,r_lagrange,r_lagrange_3] = make_problem();
% [bfu0,x_e,y_e,xearth,xmoon,yearth,ymoon,theta,r_LEO,r_LMO,G,B,map,p_0_x,p_0_y,v_0_x,v_0_y,x0,v_f]=make_hohmann();
% [f_xN_bfu] = make_f_xN_bfu(f_xnext,n,m,N,x0);
% [f_dgdu,f_g_new,f_gtil_new,f_bfx,f_dgdu_new] = make_f_g(f_xnext,f_dxn_dx,f_dxn_du,f_xN_bfu,n,m,N,x0,mu,r_LMO,v_f);
% iter_num = 10;
iter_num = 1;
% l1normlist = zeros(iter_num,1);
% u_final = struct();
% for iter=1:iter_num

epsilon = 1/10^7;

bfu = struct();% m*N=2*N 入力
bfx = struct();% n*N=4*N 状態
angle = 2;
% impulse = 600;
% bfu0 = [impulse*cos(angle*pi/4);impulse*sin(angle*pi/4);zeros(m*N-2,1)];
bfu0 = zeros(m*N,1);
% bfu0 = [12000;6000;zeros(m*N/2-2,1);-8000;9000;zeros(m*N/2-2-m*N/4,1);-1500;-500;zeros(m*N/2-4-m*N/4,1);-400;0];
% bfu0 = u_LU;
% bfu0 = u_UU;
% bfu0 = u_LL;
% bfu0 = u2;
% bfu0 = [3000;4000;zeros(m*N-2,1)];
bfu0(2) = 800;

bfu(1).data = bfu0;
bfx(1).data = f_bfx(bfu(1).data);

beta = 0.0;
c_0 = 1.0;
h = 0.0;

% beta = 0.3;
% c_0 = 1.0;
% h = 0.5;

% beta = 0.3;
% c_0 = 3.5;
% h = 0.5;

% c_max = 2*(1+beta)/(1-beta); % adaptive
% gamma = 0.95;

k = 1;
err = 1;
mmt = zeros(m*N,1);


clf;
convert_xy_vector
% plot(plot_x, plot_y, 'o', 'MarkerSize', 0.5, 'Color', 'r');
plot(plot_x, plot_y, 'o', 'MarkerSize', 0.5, 'Color', 'b');
hold on
axis equal
scatter(-mu, 0, 500, 'b', 'filled'); % 100 is the marker size
scatter(1-mu, 0, 200, 'b', 'filled'); % 100 is the marker size
% xlim([-0.2, 1.2]);
% ylim([-0.5*(1+2*mu), 0.5*(1+2*mu)]);
% xlim([-2, 2]);
% ylim([-1.2, 1.2]);
xlim([0.8, 1.3]);
ylim([-0.2, 0.2]);

axis equal;

pause(0.001)

while k<100
    % c_0 = 1 + gamma^k*(c_max-1); % adaptive
    
    sub_f_calc_w
    mmt = beta*mmt + (1-beta)*w;
    bfu(k+1).data = bfu(k).data - mmt;
    
    bfx(k+1).data = f_bfx(bfu(k+1).data);
    % err = norm(f_g_new(bfu(k+1).data),1)
    err = norm(f_xN_bfu(bfu(k+1).data) - [1-mu+r_lagrange;0;0;0],1)
    
    k=k+1
    
    convert_xy_vector
    
    plot(plot_x, plot_y, 'o', 'MarkerSize', 0.5, 'Color', 'r');
    % xlim([-0.2, 1.2]);
    % ylim([-0.5*(1+2*mu), 0.5*(1+2*mu)]);
    % xlim([-2, 2]);
    % ylim([-1.2, 1.2]);
    xlim([0.8, 1.3]);
    ylim([-0.2, 0.2]);
    
    pause(0.001)

    if err < epsilon
        residue_minimization_solver_kkt
        bfu(k).residual_l2norm = residual_l2norm;
        if residual_l2norm < 10^(-3)
            break
        end
    end
end

convert_xy_vector
plot(plot_x, plot_y, 'o', 'MarkerSize', 2, 'Color', 'k');

% close
% l1normlist(iter) = norm(bfu(k).data,1);
% u_final(iter).data = bfu(k).data;
% end